﻿#include "CalaDist_Tool.h"
/******************************************************************************
  @Project Name  : 03TestFCLCollision
  @File Name     : CalaDist_Tool.cpp
  @Version       : 1.0
  @Function      : to be determined
  @Author        : unishuai
  @Date          : 2024-05-20
******************************************************************************/

#define MY_DEBUG_INFO false

CalcDist_Tool::CalcDist_Tool()
{
    m_IsInit=false;
}

CalcDist_Tool &CalcDist_Tool::Instance()
{
    static CalcDist_Tool instance;
    if(instance.m_IsInit == false)
        instance.Init();
    return instance;
}

bool CalcDist_Tool::Init()
{
    if(m_IsInit == true)
        return true;


    m_IsInit = true;
//    printf("1111111111111111\n");
    this->initMod("./jetColObjModel/jetCol.xml");
//    printf("2222222222222222\n");
    return m_IsInit;
}

//[[deprecated("---------------------------------------")]]
void CalcDist_Tool::initMod(QString path)
{
    //    这里是读取了相应的XML的文件,顺便那那个碰撞的shape这些都给干完了
    this->readXml(path);
}

std::array<double, 5> CalcDist_Tool::checkArm2Self(int arm_index, int base_index, std::array<double, 5> angle, int check_index)
{


    std::array<double,5> error={-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}
    Eigen::VectorXd jointangle(5);
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4];
    this->RobotObjs[arm_index].updateStatus(jointangle,base_index);
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;


    std::array<double,5> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {

        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->RobotObjs[arm_index].m_ColliObj[check_index], disrequest, disresult);
        ret[i]=disresult.min_distance;
    }

    return ret;

}



std::array<double,6> CalcDist_Tool::checkArm2TunnelWall(int arm_index,int base_index,std::array<double,6> angle,int platform_index)
{
    std::array<double,6> error={-1,-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}
    if(base_index >= this->RobotObjs[arm_index].m_Base.size()){return error;}

    Eigen::VectorXd jointangle(6);
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4],angle[4];
    this->RobotObjs[arm_index].updateStatus(jointangle,base_index);
    this->TunnelWallsObjs[platform_index].updateStatus();

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    //定义的是double的最大值
    std::array<double,6> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=1;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->TunnelWallsObjs[platform_index].m_ColliObj, disrequest, disresult);
        ret[i-1]=disresult.min_distance;
    }

    return ret;
}


std::array<double,5> CalcDist_Tool::checkArm2TunnelWall(int arm_index,int base_index,std::array<double,5> angle,int platform_index)
{
    std::array<double,5> error={-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}
    if(base_index >= this->RobotObjs[arm_index].m_Base.size()){return error;}

    Eigen::VectorXd jointangle(5);
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4];
    this->RobotObjs[arm_index].updateStatus(jointangle,base_index);
    this->TunnelWallsObjs[platform_index].updateStatus();

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    //定义的是double的最大值
    std::array<double,5> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->TunnelWallsObjs[platform_index].m_ColliObj, disrequest, disresult);
        ret[i]=disresult.min_distance;
    }

    return ret;
}

std::array<double, 5> CalcDist_Tool::checkArm2TunnelWallWithPose(int arm_index, int base_index, std::array<double, 5> angle, int platform_index, std::array<double, 6> PlatformPose)
{
    std::array<double,5> error={-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}
    if(base_index >= this->RobotObjs[arm_index].m_Base.size()){return error;}

    Eigen::VectorXd jointangle(5);
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4];
    this->RobotObjs[arm_index].updateStatus(jointangle,base_index);
    //这个就是调用我修改过的函数
    this->TunnelWallsObjs[platform_index].setTransformMatrixByPosAndRotVector(PlatformPose);

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    //定义的是double的最大值
    std::array<double,5> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->TunnelWallsObjs[platform_index].m_ColliObj, disrequest, disresult);
        ret[i]=disresult.min_distance;
    }

    return ret;
}

std::array<double, 5> CalcDist_Tool::checkArmWithSlider2TunnelWallWithPose(int arm_index, int base_index, std::array<double, 5> sliderAndAngle, int platform_index, std::array<double, 6> PlatformPose)
{
    std::array<double,5> error={-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}
    if(base_index >= this->RobotObjs[arm_index].m_Base.size()){return error;}

    Eigen::VectorXd jointSliderAndAngle(5);
    jointSliderAndAngle<<sliderAndAngle[0],sliderAndAngle[1],sliderAndAngle[2],sliderAndAngle[3],sliderAndAngle[4];
    this->RobotObjs[arm_index].updateStatusBySliderAndAngle(jointSliderAndAngle,base_index);
    //这个就是调用我修改过的函数
    this->TunnelWallsObjs[platform_index].setTransformMatrixByPosAndRotVector(PlatformPose);

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    //定义的是double的最大值
    std::array<double,5> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->TunnelWallsObjs[platform_index].m_ColliObj, disrequest, disresult);
        ret[i]=disresult.min_distance;
    }

    return ret;

}

std::array<double, 6> CalcDist_Tool::checkArm2SW(int arm_index, int base_index, std::array<double, 6> angle, int cabin_index, double s_angle)
{
    std::array<double,6> error={-1,-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}
    if(base_index >= this->RobotObjs[arm_index].m_Base.size()){return error;}

    Eigen::VectorXd jointangle(6);
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4],angle[5];
    this->RobotObjs[arm_index].updateStatus(jointangle,base_index);
    this->TunnelWallsObjs[cabin_index].updateStatus(s_angle);

    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    std::array<double,6> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=1;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->TunnelWallsObjs[cabin_index].m_ColliObj, disrequest, disresult);
        ret[i-1]=disresult.min_distance;
    }

    return ret;
}

std::array<double, 7> CalcDist_Tool::checkCombinedArm2TunnelWall(int arm_index, Eigen::Matrix4d T_inst, std::array<double, 6> angle, int cabin_index)
{
    std::array<double,7> error={-1,-1,-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}

    Eigen::VectorXd jointangle(6);
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4],angle[5];
    this->RobotObjs[arm_index].updateStatus(jointangle,T_inst);
    this->TunnelWallsObjs[cabin_index].updateStatus();

    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    std::array<double,7> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->TunnelWallsObjs[cabin_index].m_ColliObj, disrequest, disresult);
        ret[i]=disresult.min_distance;
    }

    return ret;
}

std::array<double, 13> CalcDist_Tool::checkCombinedArms2TunnelWall(int arm_index, int base_index, std::array<double, 6> angle, int arm_index2, Eigen::Matrix4d T_inst, std::array<double, 6> angle2, int cabin_index)
{
    std::array<double, 6> ret1=this->checkArm2TunnelWall(arm_index,base_index,angle,cabin_index);
    Eigen::Matrix4d T_in=this->RobotObjs[arm_index].m_TransIiNow[6]*T_inst;
    std::array<double, 7> ret2=this->checkCombinedArm2TunnelWall(arm_index2,T_in,angle2,cabin_index);

    std::array<double, 13> ret;
    for(int i=0;i<13;i++)
    {
        if(i<6)
        {
            ret[i]=ret1[i];
        }
        else
        {
            ret[i]=ret2[i-6];
        }
    }
    return ret;
}

std::array<double, 7> CalcDist_Tool::checkCombinedArm2SW(int arm_index, Eigen::Matrix4d T_inst, std::array<double, 6> angle, int cabin_index, double s_angle)
{
    std::array<double,7> error={-1,-1,-1,-1,-1,-1,-1};
    if(arm_index >= this->RobotObjs.size()){return error;}

    Eigen::VectorXd jointangle(6);
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4],angle[5];
    this->RobotObjs[arm_index].updateStatus(jointangle,T_inst);
    this->TunnelWallsObjs[cabin_index].updateStatus(s_angle);

    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    std::array<double,7> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->TunnelWallsObjs[cabin_index].m_ColliObj, disrequest, disresult);
        ret[i]=disresult.min_distance;
    }

    return ret;
}

std::array<double, 13> CalcDist_Tool::checkCombinedArms2SW(int arm_index, int base_index, std::array<double, 6> angle, int arm_index2, Eigen::Matrix4d T_inst, std::array<double, 6> angle2, int cabin_index, double s_angle)
{
    std::array<double, 6> ret1=this->checkArm2SW(arm_index,base_index,angle,cabin_index,s_angle);
    Eigen::Matrix4d T_in=this->RobotObjs[arm_index].m_TransIiNow[6]*T_inst;
    std::array<double, 7> ret2=this->checkCombinedArm2SW(arm_index2,T_in,angle2,cabin_index,s_angle);

    std::array<double, 13> ret;
    for(int i=0;i<13;i++)
    {
        if(i<6)
        {
            ret[i]=ret1[i];
        }
        else
        {
            ret[i]=ret2[i-6];
        }
    }
    return ret;
}

std::vector<DistMsg> CalcDist_Tool::checkArm2Radar(int arm_index, int base_index, std::array<double, 6> angle, int radar_index, Eigen::VectorXd angle2)
{
    Eigen::VectorXd jointangle(this->RobotObjs[arm_index].m_Dof);
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof;i++)
    {
        jointangle[i]=angle[i];
    }
    jointangle<<angle[0],angle[1],angle[2],angle[3],angle[4],angle[5];
    Eigen::VectorXd radarangle(this->RadarObjs[radar_index].m_dof);
    for(int i=0;i<this->RadarObjs[radar_index].m_dof;i++)
    {
        radarangle[i]=angle2[i];
    }
    this->RobotObjs[arm_index].updateStatus(jointangle,base_index);
    this->RadarObjs[radar_index].updateStatus(radarangle);

    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;
    std::vector<DistMsg> ret;

    for(int i=1;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        for(int j=0;j<this->RadarObjs[radar_index].m_Model.size();j++)
        {
            disresult.clear();
            fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->RadarObjs[radar_index].m_ColliObj[j], disrequest, disresult);
            DistMsg temp;
            temp.distance=disresult.min_distance;
            temp.objA=this->RobotObjs[arm_index].m_Name+std::to_string(i);
            temp.objB=this->RadarObjs[radar_index].m_Name+std::to_string(j);
            temp.pointA=disresult.nearest_points[0];
            temp.pointB=disresult.nearest_points[1];
            ret.push_back(temp);
        }
    }

    std::sort(ret.begin(),ret.end(),CompareDistMsg());

    return ret;
}




std::vector<DistMsg> CalcDist_Tool::checkArmWithSlider2FlightWithAngle(int arm_index, int base_index, std::array<double, 5> SliderAndAngle, int flight_index,std::array<double,3> flightAngle)
{

    //这里就不判断错误了，我自己写的自己调用，还需要判断？
    //安全检查已经省略
    Eigen::VectorXd jointSliderAndAngle(5);
    jointSliderAndAngle<<SliderAndAngle[0],SliderAndAngle[1],SliderAndAngle[2],SliderAndAngle[3],SliderAndAngle[4];
    this->RobotObjs[arm_index].updateStatusBySliderAndAngle(jointSliderAndAngle,base_index);

    /**=================by unishuai====================
    *这个部分是新修改的
    *===================2024-09-10 ====================*/
    Eigen::VectorXd flightAngleVector(3);
    flightAngleVector<<flightAngle[0],flightAngle[1],flightAngle[2];
    this->FlightObjs[flight_index].updateStatus(flightAngleVector,0);

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);

    fcl::DistanceResultd disresult;


    //定义的是double的最大值
    //    std::array<double,5> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    std::vector<DistMsg> ret(5);
    /**=================by unishuai====================
    *这个部分也是新修改的
    *===================2024-09-10 ====================*/
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        disresult.clear();
        //因为把飞机分成了3个部分之后，会导致这里进行碰撞检测时候，需要分别对飞机的模型进行检测
        for(int j=0;j<this->FlightObjs[0].m_Dof+1;j++){
            fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj[j], disrequest, disresult);
            //滑块的名字
            std::string sliderName=this->RobotObjs[arm_index].m_Name+std::to_string(i);
            std::string flightName=this->FlightObjs[flight_index].m_Name+std::to_string(j);
            DistMsg inform(disresult.min_distance,sliderName,flightName,disresult.nearest_points[0],disresult.nearest_points[1]);
            if(j==0){
                ret[i]=inform;
            }else{
                if(ret[i].distance>inform.distance){
                    ret[i]=inform;
                }
            }

        }


        //        std::cout<<"距离点"<<disresult.nearest_points[0][0]<<","<<disresult.nearest_points[0][1]<<","<<disresult.nearest_points[0][2]<<std::endl;





    }

    return ret;
}

std::vector<DistMsg> CalcDist_Tool::checkTunnelWallWithPose2FlightWithAngle(int form_index, std::array<double, 6> PlatformPose, int flight_index, std::array<double,3> flightAngle)
{



    this->TunnelWallsObjs[form_index].setTransformMatrixByPosAndRotVector(PlatformPose);

    Eigen::VectorXd flightAngleVector(3);
    flightAngleVector<<flightAngle[0],flightAngle[1],flightAngle[2];
    this->FlightObjs[flight_index].updateStatus(flightAngleVector,0);
//    this->FlightObjs[flight_index].updateStatus(flightAngle,0);

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    //定义的是double的最大值
    std::vector<DistMsg> ret(1);
    for(int i=0;i<1;i++)
    {
        disresult.clear();
        for(int j=0;j<this->FlightObjs[0].m_Dof+1;j++){
            fcl::distance(&this->TunnelWallsObjs[form_index].m_ColliObj, &this->FlightObjs[flight_index].m_ColliObj[j], disrequest, disresult);
            //滑块的名字
            std::string sliderName=this->TunnelWallsObjs[form_index].m_Name+std::to_string(i);
            std::string flightName=this->FlightObjs[flight_index].m_Name+std::to_string(j);
            DistMsg inform(disresult.min_distance,sliderName,flightName,disresult.nearest_points[0],disresult.nearest_points[1]);
            if(j==0){
                ret[i]=inform;
            }else{
                if(ret[i].distance>inform.distance){
                    ret[i]=inform;
                }
            }

        }
//        fcl::distance(&this->TunnelWallsObjs[form_index].m_ColliObj, &this->FlightObjs[flight_index].m_ColliObj, disrequest, disresult);
//        //        ret[i]=disresult.min_distance;
//        std::string platformName=this->TunnelWallsObjs[form_index].m_Name;
//        DistMsg inform(disresult.min_distance,platformName,this->FlightObjs[flight_index].m_Name,disresult.nearest_points[0],disresult.nearest_points[1]);
//        ret[i]=inform;

    }

    return ret;

}

std::vector<DistMsg> CalcDist_Tool::checkMotionPlatformWithPose2FlightWithAngle(int motionform_index, std::array<double, 6> motionPlatformPose, std::array<double, 1> missileAngle, int flight_index, std::array<double,3> flightAngle)
{
    Eigen::VectorXd mpVectorXd(motionPlatformPose.size());
    mpVectorXd<<motionPlatformPose[0],motionPlatformPose[1],motionPlatformPose[2],motionPlatformPose[3],motionPlatformPose[4],motionPlatformPose[5];
    Eigen::VectorXd missileVectorXd(missileAngle.size());
    missileVectorXd<<missileAngle[0];
    //这个是导弹和动平台的组合模型
    this->MotionPlatformObjs[motionform_index].updateStatusByDofAndRot(mpVectorXd,missileVectorXd,0);
    Eigen::VectorXd flightAngleVector(3);
    flightAngleVector<<flightAngle[0],flightAngle[1],flightAngle[2];
    this->FlightObjs[flight_index].updateStatus(flightAngleVector,0);
//    this->FlightObjs[flight_index].updateStatus(flightAngle);

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    //定义的是double的最大值
    std::vector<DistMsg> ret(2);
    for(int i=0;i<2;i++)
    {
        disresult.clear();
        for(int j=0;j<this->FlightObjs[0].m_Dof+1;j++){
            fcl::distance(&this->MotionPlatformObjs[motionform_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj[j], disrequest, disresult);
            //滑块的名字
            std::string sliderName=this->MotionPlatformObjs[motionform_index].m_Name+std::to_string(i);
            std::string flightName=this->FlightObjs[flight_index].m_Name+std::to_string(j);
            DistMsg inform(disresult.min_distance,sliderName,flightName,disresult.nearest_points[0],disresult.nearest_points[1]);
            if(j==0){
                ret[i]=inform;
            }else{
                if(ret[i].distance>inform.distance){
                    ret[i]=inform;
                }
            }

        }
//        fcl::distance(&this->MotionPlatformObjs[motionform_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj, disrequest, disresult);
//        std::string platformName=this->MotionPlatformObjs[motionform_index].m_Name+"PART"+std::to_string(i+1);
//        DistMsg inform(disresult.min_distance,platformName,this->FlightObjs[flight_index].m_Name,disresult.nearest_points[0],disresult.nearest_points[1]);
//        ret[i]=inform;

    }

    return ret;
}

std::vector<DistMsg> CalcDist_Tool::checkMotionPlatform2TunnelWall(std::array<double,6> motionPlatformPose, std::array<double, 1> missileAngle)
{


    Eigen::VectorXd mpVectorXd(motionPlatformPose.size());
    mpVectorXd<<motionPlatformPose[0],motionPlatformPose[1],motionPlatformPose[2],motionPlatformPose[3],motionPlatformPose[4],motionPlatformPose[5];
    Eigen::VectorXd missileVectorXd(missileAngle.size());
    missileVectorXd<<missileAngle[0];
    //这个是导弹和动平台的组合模型
    this->MotionPlatformObjs[0].updateStatusByDofAndRot(mpVectorXd,missileVectorXd,0);
    //这个是静平台洞壁
    this->TunnelWallsObjs[0].updateStatus();

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);
    fcl::DistanceResultd disresult;

    std::vector<DistMsg> ret;

    for(int i=0;i<this->MotionPlatformObjs[0].m_Dof+1;i++){
        //定义的是double的最大值
        disresult.clear();
        fcl::distance(&this->MotionPlatformObjs[0].m_ColliObj[i], &this->TunnelWallsObjs[0].m_ColliObj, disrequest, disresult);

        std::string sliderName=MotionPlatformObjs[0].m_Name+"PART"+std::to_string(i+1);

        DistMsg inform(disresult.min_distance,sliderName,this->TunnelWallsObjs[0].m_Name,disresult.nearest_points[0],disresult.nearest_points[1]);

        ret.push_back(inform);
    }
    //        std::cout<<"------------"<<std::endl;
    return ret;

}

std::vector<DistMsg> CalcDist_Tool::checkArmWithSlider2ArmWithSlider(int armIndexOne, int baseIndexOne, std::vector<double> sliderOne, int armIndexTwo, int baseIndexTwo, std::vector<double> sliderTwo)
{
    //这里就不判断错误了，我自己写的自己调用，还需要判断？
    //安全检查已经省略
    Eigen::VectorXd jointOneVX(5);
    jointOneVX<<sliderOne[0],sliderOne[1],sliderOne[2],sliderOne[3],sliderOne[4];
    this->RobotObjs[armIndexOne].updateStatusBySliderAndAngle(jointOneVX,baseIndexOne);

    Eigen::VectorXd jointTwoVX(5);
    jointTwoVX<<sliderTwo[0],sliderTwo[1],sliderTwo[2],sliderTwo[3],sliderTwo[4];
    this->RobotObjs[armIndexTwo].updateStatusBySliderAndAngle(jointTwoVX,baseIndexTwo);

    /**
     * @brief disrequest
     *  enable_nearest_points_：一个布尔值，表示是否计算最近点。默认值为false。
     *  enable_signed_distance：一个布尔值，表示是否计算有符号距离。默认值为false。
     *  rel_err_：一个浮点数，表示相对误差。默认值为0.0。
     *  abs_err_：一个浮点数，表示绝对误差。默认值为0.0。
     *  distance_tolerance：一个浮点数，表示距离容差。默认值为1e-6。
     *  gjk_solver_type_：一个枚举值，表示GJK求解器的类型。默认值为GST_LIBCCD。
     */
    fcl::DistanceRequestd disrequest(true,true,0.0,0.0,1e-6,fcl::GST_LIBCCD);

    fcl::DistanceResultd disresult;


    //定义的是double的最大值
    //std::array<double,5> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    std::vector<DistMsg> ret;
    //这里我就不检测滑块和静球铰的碰撞了
    for(int i=2;i<this->RobotObjs[armIndexOne].m_Dof+1;i++)
    {
        for(int j=2;j<this->RobotObjs[armIndexTwo].m_Dof+1;j++){
            disresult.clear();
            fcl::distance(&this->RobotObjs[armIndexOne].m_ColliObj[i], &this->RobotObjs[armIndexTwo].m_ColliObj[j], disrequest, disresult);
            //        ret[i]=disresult.min_distance;

            ///这尼玛有点抽象，我不得不吐槽两句了，这里的和上面的有什么区别
            /// 怎么我重写一遍就识别成功了

            //        std::cout<<"距离点"<<disresult.nearest_points[0][0]<<","<<disresult.nearest_points[0][1]<<","<<disresult.nearest_points[0][2]<<std::endl;
            //滑块的名字
            std::string sliderNameOne=this->RobotObjs[armIndexOne].m_Name+"part"+std::to_string(i);
            std::string sliderNameTwo=this->RobotObjs[armIndexTwo].m_Name+"part"+std::to_string(j);
            DistMsg inform(disresult.min_distance,sliderNameOne,sliderNameTwo,disresult.nearest_points[0],disresult.nearest_points[1]);
            ret.push_back(inform);
        }
    }

    return ret;
}

std::vector<std::vector<DistMsg>> CalcDist_Tool::checkArmWithSliderBySideNum(int sideNum,std::vector<std::vector<double>> angles)
{
    if(sideNum>2||sideNum<0){
        std::cout<<" CalcDist_Tool::checkArmWithSliderBySideNum(int sideNum) is error!"<<std::endl;
    }
    std::vector<std::vector<DistMsg>> res;
    int sliderNum=6;
    int sliderNumHalf=sliderNum/2;
    if(sideNum==0){
        for(int i=0;i<sliderNumHalf;i++){
            for(int j=i+1;j<sliderNumHalf;j++){
                //添加两个连杆碰撞的信息
                res.push_back(checkArmWithSlider2ArmWithSlider(i, 0, angles[i], j, 0, angles[j]));
            }
        }

    }else if(sideNum==1){

        for(int i=0+sliderNumHalf;i<sliderNum;i++){
            for(int j=i+1;j<sliderNum;j++){
                //添加两个连杆碰撞的信息
                res.push_back(checkArmWithSlider2ArmWithSlider(i, 0, angles[i-sliderNumHalf], j, 0, angles[j-sliderNumHalf]));
            }
        }
    }
    return res;

}

std::vector<DistMsg> CalcDist_Tool::checkArmWithSlider2FlightWithAngleJustCol(int arm_index, int base_index, std::array<double, 5> SliderAndAngle, int flight_index, std::array<double,3> flightAngle)
{
    //这里就不判断错误了，我自己写的自己调用，还需要判断？
    //安全检查已经省略
    Eigen::VectorXd jointSliderAndAngle(5);
    jointSliderAndAngle<<SliderAndAngle[0],SliderAndAngle[1],SliderAndAngle[2],SliderAndAngle[3],SliderAndAngle[4];
    this->RobotObjs[arm_index].updateStatusBySliderAndAngle(jointSliderAndAngle,base_index);
    Eigen::VectorXd flightAngleVector(3);
    flightAngleVector<<flightAngle[0],flightAngle[1],flightAngle[2];
    this->FlightObjs[flight_index].updateStatus(flightAngleVector,0);

    //this->FlightObjs[flight_index].updateStatus(flightAngle);

    //修改成不检测距离的碰撞检测
    fcl::CollisionRequestd colrequest;
    colrequest.enable_contact = true;
    /*    colrequest.enable_contact = false;*/ // 不计算接触点信息
    colrequest.num_max_contacts = 1;
    fcl::CollisionResultd colresult;



    //定义的是double的最大值
    /**=================by unishuai====================
    *这个也是新添加的，里面J的for循环中不需要生成Information,
    * 直接判断是否进行碰撞即可
    *===================2024-09-10 ====================*/

    std::vector<DistMsg> ret(5);
    for(int i=0;i<this->RobotObjs[arm_index].m_Dof+1;i++)
    {
        colresult.clear();
        std::string sliderName=this->RobotObjs[arm_index].m_Name+std::to_string(i);
        Eigen::Vector3d v(-10000,
                          -10000,
                          -10000);

        bool isCol=false;

        for(int j=0;j<this->FlightObjs[0].m_Dof+1;j++){
            colresult.clear();
            fcl::collide(&this->RobotObjs[arm_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj[j], colrequest, colresult);
            //fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj[j], disrequest, disresult);

            //滑块的名字
            std::string sliderName=this->RobotObjs[arm_index].m_Name+std::to_string(i);
            std::string flightName=this->FlightObjs[flight_index].m_Name+std::to_string(j);
            if(colresult.isCollision()){
                isCol=true;
                const auto& contact = colresult.getContact(0);  // 获取第一个接触点
                v = contact.pos;  // 设置v为接触点位置
            }

        }




       if(isCol==true){
           DistMsg inform(0, sliderName, this->FlightObjs[flight_index].m_Name, v, v);
           ret[i] = inform;
       }else{
           DistMsg inform(10000, sliderName, this->FlightObjs[flight_index].m_Name, v, v);  // 未发生碰撞
           ret[i] = inform;
       }



    //        if(colresult.isCollision())
    //        {
    //            if (colresult.numContacts() > 0)
    //            {
    //                const auto& contact = colresult.getContact(0);  // 获取第一个接触点
    //                v = contact.pos;  // 设置v为接触点位置
    //            }
    //            DistMsg inform(0, sliderName, this->FlightObjs[flight_index].m_Name, v, v);
    //            ret[i] = inform;
    //        }
    //        else
    //        {
    //            DistMsg inform(10000, sliderName, this->FlightObjs[flight_index].m_Name, v, v);  // 未发生碰撞
    //            ret[i] = inform;
    //        }



        //        DistMsg inform(disresult.min_distance,sliderName,this->FlightObjs[flight_index].m_Name,disresult.nearest_points[0],disresult.nearest_points[1]);




    }

    return ret;

}

std::vector<DistMsg> CalcDist_Tool::checkMotionPlatform2TunnelWallJustCol(std::array<double, 6> motionPlatformPose, std::array<double, 1> missileAngle)
{

    Eigen::VectorXd mpVectorXd(motionPlatformPose.size());
    mpVectorXd<<motionPlatformPose[0],motionPlatformPose[1],motionPlatformPose[2],motionPlatformPose[3],motionPlatformPose[4],motionPlatformPose[5];
    Eigen::VectorXd missileVectorXd(missileAngle.size());
    missileVectorXd<<missileAngle[0];
    //这个是导弹和动平台的组合模型
    this->MotionPlatformObjs[0].updateStatusByDofAndRot(mpVectorXd,missileVectorXd,0);
    //这个是静平台洞壁
    this->TunnelWallsObjs[0].updateStatus();


    //修改成不检测距离的碰撞检测
    fcl::CollisionRequestd colrequest;
    colrequest.enable_contact = true; // 不计算接触点信息
    colrequest.num_max_contacts = 1;
    fcl::CollisionResultd colresult;

    std::vector<DistMsg> ret;

    for(int i=0;i<this->MotionPlatformObjs[0].m_Dof+1;i++){
        //定义的是double的最大值
        colresult.clear();

        fcl::collide(&this->MotionPlatformObjs[0].m_ColliObj[i], &this->TunnelWallsObjs[0].m_ColliObj, colrequest, colresult);

        std::string sliderName=MotionPlatformObjs[0].m_Name+"PART"+std::to_string(i+1);
        Eigen::Vector3d v(-10000,
                          -10000,
                          -10000);
        if(colresult.isCollision())
        {
            if (colresult.numContacts() > 0)
            {
                const auto& contact = colresult.getContact(0);  // 获取第一个接触点
                v = contact.pos;  // 设置v为接触点位置
            }
            DistMsg inform(0, sliderName, this->TunnelWallsObjs[0].m_Name, v, v);
            ret.push_back(inform);
        }
        else
        {
            DistMsg inform(10000, sliderName, this->TunnelWallsObjs[0].m_Name, v, v);  // 未发生碰撞
            ret.push_back(inform);
        }

//        DistMsg inform(dis,sliderName,this->TunnelWallsObjs[0].m_Name,v,v);
        //        DistMsg inform(colresult.min_distance,sliderName,this->TunnelWallsObjs[0].m_Name,disresult.nearest_points[0],disresult.nearest_points[1]);

//        ret.push_back(inform);
    }

    return ret;
}

std::vector<DistMsg> CalcDist_Tool::checkMotionPlatformWithPose2FlightWithAngleJustCol(
        int motionform_index, std::array<double, 6> motionPlatformPose, std::array<double, 1> missileAngle,
        int flight_index, std::array<double,3> flightAngle)
{
    Eigen::VectorXd mpVectorXd(motionPlatformPose.size());
    mpVectorXd<<motionPlatformPose[0],motionPlatformPose[1],motionPlatformPose[2],motionPlatformPose[3],motionPlatformPose[4],motionPlatformPose[5];
    Eigen::VectorXd missileVectorXd(missileAngle.size());
    missileVectorXd<<missileAngle[0];
    //这个是导弹和动平台的组合模型
    this->MotionPlatformObjs[motionform_index].updateStatusByDofAndRot(mpVectorXd,missileVectorXd,0);

    Eigen::VectorXd flightAngleVector(3);
    flightAngleVector<<flightAngle[0],flightAngle[1],flightAngle[2];
    this->FlightObjs[flight_index].updateStatus(flightAngleVector,0);
    //this->FlightObjs[flight_index].updateStatus(flightAngle);

    //修改成不检测距离的碰撞检测
    fcl::CollisionRequestd colrequest;
    colrequest.enable_contact = true; // 不计算接触点信息
    colrequest.num_max_contacts = 1;
    fcl::CollisionResultd colresult;

    //定义的是double的最大值
    std::vector<DistMsg> ret(6);
    for(int i=0;i<2;i++)
    {
        colresult.clear();
        std::string sliderName=this->MotionPlatformObjs[motionform_index].m_Name+"PART"+std::to_string(i+1);
        Eigen::Vector3d v(-10000,
                          -10000,
                          -10000);

        bool isCol=false;

        for(int j=0;j<this->FlightObjs[0].m_Dof+1;j++){
            colresult.clear();
            fcl::collide(&this->MotionPlatformObjs[motionform_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj[j], colrequest, colresult);
            //fcl::distance(&this->RobotObjs[arm_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj[j], disrequest, disresult);

//            //滑块的名字
//            std::string sliderName=this->MotionPlatformObjs[motionform_index].m_Name+"PART"+std::to_string(i+1);
//            std::string flightName=this->FlightObjs[flight_index].m_Name+std::to_string(j);
            if(colresult.isCollision()){
                isCol=true;
                const auto& contact = colresult.getContact(0);  // 获取第一个接触点
                v = contact.pos;  // 设置v为接触点位置
            }

        }

       if(isCol==true){
           DistMsg inform(0, sliderName, this->FlightObjs[flight_index].m_Name, v, v);
           ret[i] = inform;
       }else{
           DistMsg inform(10000, sliderName, this->FlightObjs[flight_index].m_Name, v, v);  // 未发生碰撞
           ret[i] = inform;
       }

//        fcl::collide(&this->MotionPlatformObjs[motionform_index].m_ColliObj[i], &this->FlightObjs[flight_index].m_ColliObj, colrequest, colresult);
//        //        ret[i]=disresult.min_distance;
//        std::string platformName=this->MotionPlatformObjs[motionform_index].m_Name+"PART"+std::to_string(i+1);
//        Eigen::Vector3d v(-10000,
//                          -10000,
//                          -10000);
//        if(colresult.isCollision())
//        {
//            if (colresult.numContacts() > 0)
//            {
//                const auto& contact = colresult.getContact(0);  // 获取第一个接触点
//                v = contact.pos;  // 设置v为接触点位置
//            }
//            DistMsg inform(0, platformName, this->FlightObjs[flight_index].m_Name, v, v);
//            ret.push_back(inform);
//        }
//        else
//        {
//            DistMsg inform(10000, platformName, this->FlightObjs[flight_index].m_Name, v, v);  // 未发生碰撞
//            ret.push_back(inform);
//        }


    }

    return ret;

}

std::vector<DistMsg> CalcDist_Tool::checkArmWithSlider2ArmWithSliderJustCol(
        int armIndexOne, int baseIndexOne, std::vector<double> sliderOne,
        int armIndexTwo, int baseIndexTwo, std::vector<double> sliderTwo)
{
    //这里就不判断错误了，我自己写的自己调用，还需要判断？
    //安全检查已经省略
    Eigen::VectorXd jointOneVX(5);
    jointOneVX<<sliderOne[0],sliderOne[1],sliderOne[2],sliderOne[3],sliderOne[4];
    this->RobotObjs[armIndexOne].updateStatusBySliderAndAngle(jointOneVX,baseIndexOne);

    Eigen::VectorXd jointTwoVX(5);
    jointTwoVX<<sliderTwo[0],sliderTwo[1],sliderTwo[2],sliderTwo[3],sliderTwo[4];
    this->RobotObjs[armIndexTwo].updateStatusBySliderAndAngle(jointTwoVX,baseIndexTwo);

    //修改成不检测距离的碰撞检测
    fcl::CollisionRequestd colrequest;
    colrequest.enable_contact = true; // 不计算接触点信息
    colrequest.num_max_contacts = 1;
    fcl::CollisionResultd colresult;


    //定义的是double的最大值
    //std::array<double,5> ret={DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX};
    std::vector<DistMsg> ret;
    //这里我就不检测滑块和静球铰的碰撞了
    for(int i=2;i<this->RobotObjs[armIndexOne].m_Dof+1;i++)
    {
        for(int j=2;j<this->RobotObjs[armIndexTwo].m_Dof+1;j++){
            colresult.clear();
            fcl::collide(&this->RobotObjs[armIndexOne].m_ColliObj[i], &this->RobotObjs[armIndexTwo].m_ColliObj[j], colrequest, colresult);
            //滑块的名字
            std::string sliderNameOne=this->RobotObjs[armIndexOne].m_Name+"part"+std::to_string(i);
            std::string sliderNameTwo=this->RobotObjs[armIndexTwo].m_Name+"part"+std::to_string(j);
            Eigen::Vector3d v(-10000,
                              -10000,
                              -10000);
            if(colresult.isCollision())
            {
                if (colresult.numContacts() > 0)
                {
                    const auto& contact = colresult.getContact(0);  // 获取第一个接触点
                    v = contact.pos;  // 设置v为接触点位置
                }
                DistMsg inform(0, sliderNameOne, sliderNameTwo, v, v);
                ret.push_back(inform);
            }
            else
            {
                DistMsg inform(10000, sliderNameOne, sliderNameTwo, v, v);  // 未发生碰撞
                ret.push_back(inform);
            }
//            int dis=colresult.isCollision() ==true?0:10000;
//            DistMsg inform(dis,sliderNameOne,sliderNameTwo,v,v);
            //                DistMsg inform(disresult.min_distance,sliderNameOne,sliderNameTwo,disresult.nearest_points[0],disresult.nearest_points[1]);
//            ret.push_back(inform);
        }
    }

    return ret;

}

std::vector<std::vector<DistMsg> > CalcDist_Tool::checkArmWithSliderBySideNumJustCol(int sideNum, std::vector<std::vector<double> > angles)
{
    if(sideNum>2||sideNum<0){
        std::cout<<" CalcDist_Tool::checkArmWithSliderBySideNum(int sideNum) is error!"<<std::endl;
    }
    std::vector<std::vector<DistMsg>> res;
    int sliderNum=6;
    int sliderNumHalf=sliderNum/2;
    if(sideNum==0||sideNum==2){
        for(int i=0;i<sliderNumHalf;i++){
            for(int j=i+1;j<sliderNumHalf;j++){
                //添加两个连杆碰撞的信息
                res.push_back(checkArmWithSlider2ArmWithSliderJustCol(i, 0, angles[i], j, 0, angles[j]));
            }
        }

    }else if(sideNum==1||sideNum==2){

        for(int i=0+sliderNumHalf;i<sliderNum;i++){
            for(int j=i+1;j<sliderNum;j++){
                //添加两个连杆碰撞的信息
                res.push_back(checkArmWithSlider2ArmWithSliderJustCol(i, 0, angles[i-sliderNumHalf], j, 0, angles[j-sliderNumHalf]));
            }
        }
    }
    return res;
}





void CalcDist_Tool::loadOBJFile(const char *filename, std::vector<fcl::Vector3d> &points, std::vector<fcl::Triangle> &triangles)
{
    FILE* file = fopen(filename, "rb");
    if (!file)
    {
        std::cerr << "file not exist" << std::endl;
        return;
    }

    bool has_normal = false;
    bool has_texture = false;
    char line_buffer[2000];
    //这里是实现了obj文件的读取
    //捏麻麻，这不是坑人吗，这个fgets只是读取一行
    while (fgets(line_buffer, 2000, file))
    {
        //这玩意分割字符串用的，类似于正则表达
        char* first_token = strtok(line_buffer, "\r\n\t ");
        if (!first_token || first_token[0] == '#' || first_token[0] == 0)
            continue;

        switch (first_token[0])
        {
        //这里是顶点
        case 'v':
        {
            //法线
            if (first_token[1] == 'n')
            {
                strtok(nullptr, "\t ");
                strtok(nullptr, "\t ");
                strtok(nullptr, "\t ");
                has_normal = true;
            }
            //纹理
            else if (first_token[1] == 't')
            {
                strtok(nullptr, "\t ");
                strtok(nullptr, "\t ");
                has_texture = true;
            }
            //这里就是正常的顶点逻辑
            else
            {
                double x = (double)atof(strtok(nullptr, "\t "));
                double y = (double)atof(strtok(nullptr, "\t "));
                double z = (double)atof(strtok(nullptr, "\t "));
                points.emplace_back(x, y, z);
            }
        }
            break;
            //这里就是三角形片面数据
        case 'f':
        {
            fcl::Triangle tri;
            char* data[30];
            int n = 0;
            while ((data[n] = strtok(nullptr, "\t \r\n")) != nullptr)
            {
                if (strlen(data[n]))
                    n++;
            }

            for (int t = 0; t < (n - 2); ++t)
            {
                //因为一般都是从0开始的，obj格式里面的从1开始
                if ((!has_texture) && (!has_normal))
                {
                    tri[0] = atoi(data[0]) - 1;
                    tri[1] = atoi(data[1]) - 1;
                    tri[2] = atoi(data[2]) - 1;
                }
                else
                {
                    const char *v1;
                    for (int i = 0; i < 3; i++)
                    {
                        // vertex ID
                        if (i == 0)
                            v1 = data[0];
                        else
                            v1 = data[t + i];

                        tri[i] = atoi(v1) - 1;
                    }
                }
                triangles.push_back(tri);
            }
        }
        }
    }
    fclose(file);
}

void CalcDist_Tool::readXml(QString path)
{
    this->RobotObjs.clear();
    this->TunnelWallsObjs.clear();
    this->RadarObjs.clear();
    this->FlightObjs.clear();
    this->MotionPlatformObjs.clear();

    QString filename = path;
    //这里是打开文件
    QFile file(filename);
    if(!file.open(QFile::ReadOnly))
    {
        return ;
    }
    //创建dom
    QDomDocument doc;
    //看看是不是能解析
    if(!doc.setContent(&file))
    {
        file.close();
        return ;
    }
    //关闭文件
    file.close();

    //获取文档的根节点
    QDomElement root = doc.documentElement();

    //获取文档的第一个子节点，这里的话，应该获取的是
    //    <properties>
    //            <ModelPath>./ObjModel</ModelPath>
    //            <objNum>2</objNum>
    //    </properties>
    QDomNode properties = root.firstChild();//1

    //  这就是properties中<ModelPath>./ObjModel</ModelPath>这一行，然后取出它的Text元素
    QString objpath = properties.firstChild().toElement().text();
    //    这里获取的是<ModelPath>的下一个兄弟节点，也就是<objNum>2</objNum>，取出来值为2
    int obj_num = properties.firstChild().nextSibling().toElement().text().toInt();


    for(QDomNode obj=properties.nextSibling(); !obj.isNull(); obj = obj.nextSibling())
    {
        //这里我就按照第一轮去分析
        //<OBJ0>...</OBJ0>
        //第一个是name标签
        QDomNode name=obj.firstChild();
        //标签里面的信息
        QString tempname=name.toElement().text();
        //转化为标准的String
        std::string temp_name=tempname.toStdString();


        //获取类型
        QDomNode type=obj.firstChild().nextSibling();
        int i=type.toElement().text().toInt();


        //这里就是一个switch
        if(i==0)
        {
            if(MY_DEBUG_INFO){
                std::cout<<"enter i==0"<<std::endl;
            }

            int basenum=type.nextSibling().toElement().text().toInt();
            int dof=type.nextSibling().nextSibling().toElement().text().toInt();

            std::vector<std::shared_ptr<Model>> modellist;
            for(int i=0;i<dof+1;i++)
            {
                QString _path_=objpath+"/"+tempname+"/"+QString::number(i)+".obj";
                QByteArray _path=_path_.toLatin1();
                const char* path=_path.data();

                std::vector<fcl::Vector3d> p1;
                std::vector<fcl::Triangle> t1;
                //这里就是按照顺序加载了我的模型
                loadOBJFile(path,p1,t1);
                //下面是model这个类的全称
                //Bounding Volume Hierarchy Model with Object-Bounding Box and Relative Oriented Bounding Box Split Strategy"
                std::shared_ptr<Model> geom1 = std::make_shared<Model>();
                geom1->beginModel();
                geom1->addSubModel(p1, t1);
                geom1->endModel();

                modellist.push_back(geom1);


            }

            std::vector<double> T;
            T.clear();

            std::vector<Eigen::Matrix4d> T_arm;
            T_arm.clear();

            /**
                <BaseTrans>
                    <POSX>0</POSX>
                    <POSY>0</POSY>
                    <POSZ>0</POSZ>
                    <ROTX>0</ROTX>
                    <ROTY>0</ROTY>
                    <ROTZ>0</ROTZ>
                </BaseTrans>
            **/
            for(QDomNode PE=type.nextSibling().nextSibling().nextSibling(); !PE.isNull(); PE = PE.nextSibling())
            {
                for(QDomNode angle=PE.firstChild(); !angle.isNull(); angle = angle.nextSibling())
                {
                    T.push_back(angle.toElement().text().toDouble());
                }
                //这里就是告诉你position 和 rotation
                T_arm.push_back(RobotBasic::PE2T(T[0],T[1],T[2],T[3],T[4],T[5]));
                T.clear();
            }

            std::vector<Eigen::Matrix4d> T_base;
            std::vector<Eigen::Matrix4d> T_ini;

            //这里就是baseNum
            for(int j=0;j<basenum;j++)
            {
                T_base.push_back(T_arm[j]);
            }
            //这里应该就是手臂了吧(不懂这里为什么要用size(),因为我看他只有六个自由度，但是有一个BaseTrans和7个partTrans)
            for(int j=basenum;j<T_arm.size();j++)
            {
                T_ini.push_back(T_arm[j]);
            }

            //终于把这个初始化看完了
            class RobotObj temprobot(temp_name, dof, modellist, T_base, T_ini);

            //把机器人推进去了
            this->RobotObjs.push_back(temprobot);
            if(MY_DEBUG_INFO){
                std::cout<<"outer i==0"<<std::endl;
            }

        }

        else if (i==1)
        {
            if(MY_DEBUG_INFO){
                std::cout<<"enter i==1"<<std::endl;
            }


            std::vector<double> T;
            T.clear();
            Eigen::Matrix4d T_cabin;
            //这里的雷达只有一个，所以不需要去整个for循环
            QString _path_=objpath+"/"+tempname+"/"+"0.obj";
            QByteArray _path=_path_.toLatin1();
            const char* path=_path.data();

            std::vector<fcl::Vector3d> p1;
            std::vector<fcl::Triangle> t1;
            //这里就是把模型加载进来了，然后通过obj文件，读取出来了点和网格面
            loadOBJFile(path,p1,t1);

            std::shared_ptr<Model> geom1 = std::make_shared<Model>();
            geom1->beginModel();
            geom1->addSubModel(p1, t1);
            geom1->endModel();

            for(QDomNode PE=type.nextSibling(); !PE.isNull(); PE = PE.nextSibling())
            {
                for(QDomNode angle=PE.firstChild(); !angle.isNull(); angle = angle.nextSibling())
                {
                    T.push_back(angle.toElement().text().toDouble());
                }
            }

            T_cabin=RobotBasic::PE2T(T[0],T[1],T[2],T[3],T[4],T[5]);
            T.clear();
            //这里的这个地方和上面是差不多的,但还是机器人稍微复杂一点
            class TunnelWallObj tempcabin(temp_name, geom1, T_cabin);
            this->TunnelWallsObjs.push_back(tempcabin);
            if(MY_DEBUG_INFO){
                std::cout<<"outer i==1"<<std::endl;
            }

        }
        else if (i==2)
        {
            if(MY_DEBUG_INFO){
                std::cout<<"enter i==2"<<std::endl;
            }

            int dof=type.nextSibling().toElement().text().toInt();
            std::vector<std::shared_ptr<Model>> modellist;
            for(int i=0;i<dof;i++)
            {
                QString _path_=objpath+"/"+tempname+"/"+QString::number(i)+".obj";
                QByteArray _path=_path_.toLatin1();
                const char* path=_path.data();

                std::vector<fcl::Vector3d> p1;
                std::vector<fcl::Triangle> t1;
                loadOBJFile(path,p1,t1);

                std::shared_ptr<Model> geom1 = std::make_shared<Model>();
                geom1->beginModel();
                geom1->addSubModel(p1, t1);
                geom1->endModel();

                modellist.push_back(geom1);
            }

            std::vector<double> T;
            T.clear();

            std::vector<Eigen::Matrix4d> Trans;
            Trans.clear();

            for(QDomNode PE=type.nextSibling().nextSibling(); !PE.isNull(); PE = PE.nextSibling())
            {
                for(QDomNode angle=PE.firstChild(); !angle.isNull(); angle = angle.nextSibling())
                {
                    T.push_back(angle.toElement().text().toDouble());
                }

                Trans.push_back(RobotBasic::PE2T(T[0],T[1],T[2],T[3],T[4],T[5]));
                T.clear();
            }

            class RadarObj tempradar(temp_name,dof,modellist,Trans);
            this->RadarObjs.push_back(tempradar);
            if(MY_DEBUG_INFO){
                std::cout<<"outer i==2"<<std::endl;
            }

        }
        else if (i==3)
        {

            //三号是飞机，
            //由于飞机增加了自由度，目前不像单自由度的雷达
            //这里我修改了类的类型，把它修改成了类似于机器人的格式
            if(MY_DEBUG_INFO){
                std::cout<<"enter i==3"<<std::endl;
            }

            int basenum=type.nextSibling().toElement().text().toInt();
            int dof=type.nextSibling().nextSibling().toElement().text().toInt();

            std::vector<std::shared_ptr<Model>> modellist;

            for(int i=0;i<dof+1;i++)
            {
                QString _path_=objpath+"/"+tempname+"/"+QString::number(i)+".obj";
                QByteArray _path=_path_.toLatin1();
                const char* path=_path.data();

                std::vector<fcl::Vector3d> p1;
                std::vector<fcl::Triangle> t1;
                //这里就是按照顺序加载了我的模型
                loadOBJFile(path,p1,t1);
                //下面是model这个类的全称
                //Bounding Volume Hierarchy Model with Object-Bounding Box and Relative Oriented Bounding Box Split Strategy"
                std::shared_ptr<Model> geom1 = std::make_shared<Model>();

                geom1->beginModel();
                geom1->addSubModel(p1, t1);
                geom1->endModel();

                modellist.push_back(geom1);


            }


            std::vector<double> T;
            T.clear();

            std::vector<Eigen::Matrix4d> T_arm;
            T_arm.clear();

            /**
                <BaseTrans>
                    <POSX>0</POSX>
                    <POSY>0</POSY>
                    <POSZ>0</POSZ>
                    <ROTX>0</ROTX>
                    <ROTY>0</ROTY>
                    <ROTZ>0</ROTZ>
                </BaseTrans>
            **/
            for(QDomNode PE=type.nextSibling().nextSibling().nextSibling(); !PE.isNull(); PE = PE.nextSibling())
            {
                for(QDomNode angle=PE.firstChild(); !angle.isNull(); angle = angle.nextSibling())
                {
                    T.push_back(angle.toElement().text().toDouble());
                }
                //这里就是告诉你position 和 rotation
                T_arm.push_back(RobotBasic::PE2T(T[0],T[1],T[2],T[3],T[4],T[5]));
                T.clear();
            }

            std::vector<Eigen::Matrix4d> T_base;
            std::vector<Eigen::Matrix4d> T_ini;

            //这里就是baseNum
            for(int j=0;j<basenum;j++)
            {
                T_base.push_back(T_arm[j]);
            }
            //这里应该就是手臂了吧(不懂这里为什么要用size(),因为我看他只有六个自由度，但是有一个BaseTrans和7个partTrans)
            for(int j=basenum;j<T_arm.size();j++)
            {
                T_ini.push_back(T_arm[j]);
            }

            //终于把这个初始化看完了
            class FlightObj temproflight(temp_name, dof, modellist, T_base, T_ini);

            //把机器人推进去了
            this->FlightObjs.push_back(temproflight);
            if(MY_DEBUG_INFO){
                std::cout<<"outer i==3"<<std::endl;
            }


        }
        else if (i==4)  {
            if(MY_DEBUG_INFO){
                std::cout<<"enter i==4"<<std::endl;
            }

            //这个就是动平台，我把它单独拿了出来，
            //动平台这玩意有六个自由度，然后最开始的时候，只有一个模型，所以六个自由度直接设置就行，但是后面这个动平台和导弹分开了，
            //所以还必须要保证导弹和动平台的相对的距离不会有变化，然后导弹还有旋转，我觉得还是有些许抽象的
            int basenum=type.nextSibling().toElement().text().toInt();
            int dof=type.nextSibling().nextSibling().toElement().text().toInt();

            std::vector<std::shared_ptr<Model>> modellist;
            for(int i=0;i<dof+1;i++)
            {
                QString _path_=objpath+"/"+tempname+"/"+QString::number(i)+".obj";
                QByteArray _path=_path_.toLatin1();
                const char* path=_path.data();

                std::vector<fcl::Vector3d> p1;
                std::vector<fcl::Triangle> t1;
                //这里就是按照顺序加载了我的模型
                loadOBJFile(path,p1,t1);
                //下面是model这个类的全称
                //Bounding Volume Hierarchy Model with Object-Bounding Box and Relative Oriented Bounding Box Split Strategy"
                std::shared_ptr<Model> geom1 = std::make_shared<Model>();
                geom1->beginModel();
                geom1->addSubModel(p1, t1);
                geom1->endModel();

                modellist.push_back(geom1);
            }

            std::vector<double> T;
            T.clear();

            std::vector<Eigen::Matrix4d> T_arm;
            T_arm.clear();

            /**
                <BaseTrans>
                    <POSX>0</POSX>
                    <POSY>0</POSY>
                    <POSZ>0</POSZ>
                    <ROTX>0</ROTX>
                    <ROTY>0</ROTY>
                    <ROTZ>0</ROTZ>
                </BaseTrans>
            **/
            for(QDomNode PE=type.nextSibling().nextSibling().nextSibling(); !PE.isNull(); PE = PE.nextSibling())
            {
                for(QDomNode angle=PE.firstChild(); !angle.isNull(); angle = angle.nextSibling())
                {
                    T.push_back(angle.toElement().text().toDouble());
                }
                //这里就是告诉你position 和 rotation
                T_arm.push_back(RobotBasic::PE2T(T[0],T[1],T[2],T[3],T[4],T[5]));
                T.clear();
            }

            std::vector<Eigen::Matrix4d> T_base;
            std::vector<Eigen::Matrix4d> T_ini;

            //这里就是baseNum
            for(int j=0;j<basenum;j++)
            {
                T_base.push_back(T_arm[j]);
            }
            //这里应该就是手臂了吧(不懂这里为什么要用size(),因为我看他只有六个自由度，但是有一个BaseTrans和7个partTrans)
            for(int j=basenum;j<T_arm.size();j++)
            {
                T_ini.push_back(T_arm[j]);
            }

            //终于把这个初始化看完了
            class MotionPlatformObj temprobot(temp_name, dof, modellist, T_base, T_ini);

            //把机器人推进去了
            this->MotionPlatformObjs.push_back(temprobot);
            if(MY_DEBUG_INFO){
                std::cout<<"outer i==4"<<std::endl;
            }


        }
    }

    return;
}


std::string DistMsg::getObjA() const
{
    return objA;
}

void DistMsg::setObjA(const std::string &value)
{
    objA = value;
}

std::string DistMsg::getObjB() const
{
    return objB;
}

void DistMsg::setObjB(const std::string &value)
{
    objB = value;
}

Eigen::Vector3d DistMsg::getPointA() const
{
    return pointA;
}

void DistMsg::setPointA(const Eigen::Vector3d &value)
{
    pointA = value;
}

Eigen::Vector3d DistMsg::getPointB() const
{
    return pointB;
}

void DistMsg::setPointB(const Eigen::Vector3d &value)
{
    pointB = value;
}

double DistMsg::getDistance() const
{
    return distance;
}

void DistMsg::setDistance(double value)
{
    distance = value;
}
